using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using Microsoft.Ccr.Core;
using System.IO;
using ccr = Microsoft.Ccr.Core;
using System.Threading;


namespace VEXRobotics
{
    #region Serial Port Data Structures
    public class SerialPortConfig
    {
        private int _commPort;
        private int _baudRate;
        private Parity _parity = Parity.None;
        private int _dataBits = 8;
        private StopBits _stopBits = StopBits.One;
        private string _portName;

        /// <summary>
        /// Serial Communications Port 
        /// </summary>
        public int CommPort
        {
            get { return _commPort; }
            set { _commPort = value; }
        }

        /// <summary>
        /// Port name
        /// </summary>
        public string PortName
        {
            get { return _portName; }
            set { _portName = value; }
        }

        /// <summary>
        /// Baud Rate
        /// </summary>
        public int BaudRate
        {
            get { return _baudRate; }
            set { _baudRate = value; }
        }

        /// <summary>
        /// Specifies the Parity bit for a Serial Port.
        /// </summary>
        public Parity Parity
        {
            get { return _parity; }
            set { _parity = value; }
        }

        /// <summary>
        /// Number of data bits
        /// </summary>
        public int DataBits
        {
            get { return _dataBits; }
            set { _dataBits = value; }
        }

        /// <summary>
        /// Number of stop bits
        /// </summary>
        public StopBits StopBits
        {
            get { return _stopBits; }
            set { _stopBits = value; }
        }
    }

    #endregion

    /// <summary>
    /// Connection to the robot
    /// </summary>
    internal class VEXConnection
    {
        /// <summary>
        /// Valid baud rates
        /// </summary>
        int[] baudRates = { 115200, 9600 };

        SerialPortConfig config = new SerialPortConfig();
        SerialPort serialPort = new System.IO.Ports.SerialPort();
        VEXService _vexService;

        /// <summary>
        /// Default constructor
        /// </summary>
        /// <param name="vex">Service to receive responses</param>
        public VEXConnection(VEXService vex)
        {
            _vexService = vex;
        }

        /// <summary>
        /// Open a serial port.
        /// </summary>
        /// <param name="comPort">COM port to use</param>
        /// <param name="baudRate">Requested baud rate</param>
        /// <returns>A Ccr Port for receiving serial port data</returns>
        public bool Open(int comPort, int baudRate)
        {
            if (baudRate < 1200)
                baudRate = 115200;

            Close();

            string portName = "COM" + comPort.ToString(System.Globalization.NumberFormatInfo.InvariantInfo);
            if (serialPort == null)
            {
                serialPort = new SerialPort(portName, baudRate);
            }
            else
            {
                serialPort.PortName = portName;
                serialPort.BaudRate = baudRate;
            }
            serialPort.Encoding = Encoding.Default;
            serialPort.Parity = Parity.None;
            serialPort.DataBits = 8;
            serialPort.StopBits = StopBits.One;
            serialPort.WriteTimeout = 2000;
            serialPort.ReadTimeout = 2000;

            try
            {
                serialPort.Open();

                // Wait for the robot to initialize
                System.Threading.Thread.Sleep(2000);
            }
            catch
            {
                _vexService.LogConsoleError("Invalid Serial Port.");
                ShowVEXHelp(_vexService);
                return false;
            }
            return true;
        }

        /// <summary>
        /// Send a command to the robot
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        internal VEXResponse SendCommand(VEXCommand cmd)
        {
            // Make sure the connection is valid
            VEXResponse response = null;
            if (serialPort == null || !serialPort.IsOpen)
                return response;

            // Make sure the data is valid
            if (cmd.Data != null && cmd.Data.Length > 64)
                return response;

            // Start with the preamble
            byte[] buffer;
            buffer = new byte[128];
            buffer[0] = (byte)'$';
            buffer[1] = (byte)'$';

            // Checksum
            buffer[2] = 0;

            // Length of the data
            buffer[3] = (byte)((cmd.Data == null) ? 0 : cmd.Data.Length);

            // Add the data packet
            int ix = 4;
            buffer[ix++] = (byte) cmd.VEXCommandCode;
            if (cmd.Data != null && cmd.Data.Length > 0)
            {
                foreach (byte b in cmd.Data)
                {
                    buffer[ix++] = b;
                }
            }

            // null terminate for good measure
            buffer[ix] = 0;

            // Now finally calculate the checksum
            for (int iy = 2; iy < ix; ++iy)
            {
                buffer[2] ^= buffer[iy];
            }

            // Send the buffer
            try
            {
                serialPort.Write(buffer, 0, ix);

                // Get the response
                response = GetCommandResponse(cmd);
                return response;
            }
            catch (System.IO.IOException t)
            {
                return response;
            }
            catch (TimeoutException t)
            {
                return response;
            }
        }


        /// <summary>
        /// Display VEX Help to the console
        /// </summary>
        public static void ShowVEXHelp(VEXService vexService)
        {
            vexService.LogConsoleError("Please make sure the VEX is turned on, connected\r\n"
                + "    via Bluetooth, and configured to the proper port.\r\n");
        }

        /// <summary>
        /// Something went wrong...
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void serialPort_ErrorReceived(object sender, SerialErrorReceivedEventArgs e)
        {
            throw new Exception("The method or operation is not implemented.");
        }

        /// <summary>
        /// Wait for a response
        /// </summary>
        /// <param name="ticks"></param>
        /// <param name="numChars"></param>
        /// <returns></returns>
        private bool WaitForResponse(int ticks, int numChars)
        {
            while (serialPort.BytesToRead < numChars && ticks > 0)
            {
                ticks--;
                System.Threading.Thread.Sleep(100);
            }
            return serialPort.BytesToRead < numChars;
        }

        /// <summary>
        /// Read Serial Port for a VEX response
        /// </summary>
        /// <param name="cmd"></param>
        private VEXResponse GetCommandResponse(VEXCommand cmd)
        {
            if (cmd != null && serialPort != null && serialPort.IsOpen)
            {
                try
                {
                    // Skip the header
                    if(WaitForResponse(20, 2))
                    {
                        // Garbled response.  
                        // Wait for remaining bytes
                        // return an error
                        System.Threading.Thread.Sleep(500);
                        serialPort.DiscardInBuffer();
                        IOException ex1 = new IOException("No data received from VEX.");
                        return new VEXResponseException(cmd, ex1);
                    }

                    int ch = serialPort.ReadByte();
                    if (ch == '$')
                    {
                        ch = serialPort.ReadByte();
                        if (ch != (int) '$')
                        {
                            // Garbled response.  
                            // Wait for remaining bytes
                            // return an error
                            System.Threading.Thread.Sleep(500);
                            serialPort.DiscardInBuffer();
                            IOException ex1 = new IOException("Garbled data received from VEX.  Preamble corrupted.");
                            return new VEXResponseException(cmd, ex1);
                        }
                    }

                    // Wait for the header
                    // Header format
                    // byte     packet length
                    // byte     checksum
                    // byte     command
                    //
                    if (WaitForResponse(20, 3))
                    {
                        // Garbled response.  
                        // Wait for remaining bytes
                        // return an error
                        System.Threading.Thread.Sleep(500);
                        serialPort.DiscardInBuffer();
                        IOException ex1 = new IOException("Insufficient data received from VEX.");
                        return new VEXResponseException(cmd, ex1);
                    }
                    VEXHelper.VEXCommandCode command = (VEXHelper.VEXCommandCode)serialPort.ReadByte();
                    int btLength = serialPort.ReadByte();
                    byte checksum = (byte)serialPort.ReadByte();

                    // Make sure the expected data length matches
                    int dataLength = VEXHelper.ReturnPacketDataSize(command);
                    if (dataLength != btLength)
                    {
                        // Garbled response.  
                        // Wait for remaining bytes
                        // return an error
                        System.Threading.Thread.Sleep(500);
                        serialPort.DiscardInBuffer();
                        IOException ex1 = new IOException("Garbled data received from VEX.  Response size is not size expected.");
                        return new VEXResponseException(cmd, ex1);
                    }
                    VEXResponse vexReceive = new VEXResponse(command, dataLength);

                    // If we need to read a response, read it
                    if (btLength > 0)
                    {
                        if (WaitForResponse(20, btLength))
                        {
                            // Garbled response.  
                            // Wait for remaining bytes
                            // return an error
                            System.Threading.Thread.Sleep(500);
                            serialPort.DiscardInBuffer();
                            IOException ex1 = new IOException("Garbled data received from VEX.  Not all of the response was recieved.");
                            return new VEXResponseException(cmd, ex1);
                        }

                        // Read the data block
                        vexReceive.Data = new byte[btLength];
                        dataLength = serialPort.Read(vexReceive.Data, 0, btLength);

                        // Make we read what we should have
                        if (dataLength != btLength)
                        {
                            // Garbled response.  
                            // Wait for remaining bytes
                            // return an error
                            System.Threading.Thread.Sleep(500);
                            serialPort.DiscardInBuffer();
                            IOException ex1 = new IOException("Garbled data received from VEX.  Response size recieved does not match expected.");
                            return new VEXResponseException(cmd, ex1);
                        }
                    }
                    return vexReceive;
                }
                catch (ArgumentException ex)
                {
                    if (ex.Message == "Offset and length were out of bounds for the array or count is greater than the number of elements from index to the end of the source collection.")
                    {
                        _vexService.LogConsoleError("A connection error occured which may be caused by an invalid Baud Rate");
                        IOException ex3 = new IOException("Invalid Baud Rate");
                        return new VEXResponseException(cmd, ex3);
                    }
                    else
                    {
                        _vexService.LogConsoleError("A connection error occured while accessing the VEX serial port: " + ex.Message);
                        return new VEXResponseException(cmd, ex);
                    }
                }
                catch (TimeoutException ex)
                {
                    // Ignore timeouts for now.
                    _vexService.LogConsoleInfo("Timeout reading from VEX");
                    return new VEXResponseException(cmd, ex);
                }
                catch (IOException ex)
                {
                    _vexService.LogConsoleError(string.Format("Error reading from the serial port in CommBase(): {0}", ex.Message));
                    return new VEXResponseException(cmd, ex);
                }
                catch (Exception ex)
                {
                    _vexService.LogConsoleError(string.Format("Error reading from the serial port in CommBase(): {0}", ex.Message));
                    return new VEXResponseException(cmd, ex);
                }
            }

            VEXResponse response = new VEXResponse(cmd.VEXCommandCode, 1);
            if (cmd == null)
            {
                if (cmd == null)
                {
                    _vexService.LogConsoleError("Invalid VEX Command: null");
                    return new VEXResponseException(cmd, "Invalid VEX Command: null");
                }
                else if (serialPort == null || !serialPort.IsOpen)
                {
                    _vexService.LogConsoleWarning(string.Format("Unable to retrieve response from VEX command {0} because the serial port is not open.", cmd.VEXCommandCode));
                    return new VEXResponseException(cmd, "Unable to retrieve response from VEX command {0} because the serial port is not open.");
                }
            }

            return response;
        }

        /// <summary>
        /// Close the connection to a serial port.
        /// </summary>
        public void Close()
        {
            if (serialPort != null)
            {
                if (serialPort.IsOpen)
                {
                    serialPort.Close();
                    System.Threading.Thread.Sleep(1000);
                }

                serialPort = null;
            }
        }

        /// <summary>
        /// True when the serial port connection to the VEX is open.
        /// </summary>
        public bool IsOpen
        {
            get { return serialPort != null && serialPort.IsOpen; }
        }
    }

    internal class VEXDataPort : PortSet<
        VEXResponse, 
        VEXResponseGetAllInput, 
        Exception>
    {
    }
}
